//
//  Robot.m
//  NXTRemoteController
//
//  Created by naceka on 24.03.12.
//  Copyright 2012 __MyCompanyName__. All rights reserved.
//

#import "Robot.h"


@implementation Robot

- (Robot *) init {
	self = [super init];
	
	//create instance of BtManager
	manager = [BTstackManager sharedInstance];
	[manager setDelegate:self];
	
	//clean sensors
	for (int i = 0; i < 4; i++) {
		sensors[i] = nil;
	}
	
	speed = 0;
	timer = nil;
	
	return self;
}

//set of NXT id
- (void) setNxtId:(bd_addr_t *) addr {
	device_address = addr;
}

- (NSString *) getNxtId {
	NSMutableArray * addr = [NSMutableArray arrayWithCapacity:6];
	
	for (int i = 0; i < 6; i++) {
		//add 0x
		if ((unsigned char)((*device_address)[i]) < 0x10) {
			[addr addObject:[NSString stringWithFormat:@"0%X", (unsigned char)((*device_address)[i])]];
		}
		else {
			[addr addObject:[NSString stringWithFormat:@"%2X", (unsigned char)((*device_address)[i])]];
		}
	}
	
	NSString * address = [NSString stringWithFormat:@"%@:%@:%@:%@:%@:%@", [addr objectAtIndex:0], [addr objectAtIndex:1], [addr objectAtIndex:2], [addr objectAtIndex:3], [addr objectAtIndex:4], [addr objectAtIndex:5]];
	return address;
}

//register of sensor type method
- (void) registerSensor:(Sensor *) sensor OnPort:(int) port{

	//check number of port
	if (port >= 0 && port < 4) {
		sensors[port] = sensor;	
	}
}

//register motor method
- (void) registerLeftMotor:(enum Motor) motor {
	leftMotor = motor;
}

- (void) registerRightMotor:(enum Motor) motor {
	rightMotor = motor;
}

//connection methods
- (void) connect {
	NSLog(@"connect!");			
	if ([manager activated]) {
		[manager createRFCOMMConnectionAtAddress:device_address withChannel:1 authenticated:NO];
		NSLog(@"manager activated - new connection");
	}
	else {
		[manager activate];
		NSLog(@"manager activate");
	}
}

- (void) disconnect {
	[manager closeRFCOMMConnection];
}

//speed method
- (void) setSpeed:(int) _sp {
	speed = (char)_sp;
}

//move methods
- (void) moveForward {
	[self moveLeft:speed andRight:speed];		
}

- (void) moveBackward {
	[self moveLeft:-speed andRight:-speed];
}

- (void) moveLeft {
	[self moveLeft: (char) 0 andRight:speed];
}

- (void) moveRight {
	[self moveLeft:speed andRight: (char) 0];
}

- (void) stop {
	[self moveLeft:'\0' andRight:'\0'];
}

- (void) moveLeft:(char) lspeed andRight:(char) rspeed {
	char command[28];
	
	command[0] = 0x0c;
	command[1] = 0x00;
	command[2] = 0x80;	//
	command[3] = 0x04;	//set output state
	command[4] = (char)leftMotor;	//port - left motor
	command[5] = lspeed;	//speed
	command[6] = 0x01;	//regulated ?
	command[7] = 0x01;	//regulation mode motor idle
	command[8] = 0x00;	//turn ratio 0
	command[9] = 0x20;	//MOTOR_RUN_STATE_RUNNING
	command[10] = 0x00;//run forever
	command[11] = 0x00;//run forever
	command[12] = 0x00;//run forever
	command[13] = 0x00;//run forever
	
	command[14] = 0x0c;
	command[15] = 0x00;
	command[16] = 0x80;	//
	command[17] = 0x04;	//set output state
	command[18] = (char)rightMotor;	//port - right motor
	command[19] = rspeed;	//speed
	command[20] = 0x01;	//regulated ?
	command[21] = 0x01;	//regulation mode motor idle
	command[22] = 0x00;	//turn ratio 0
	command[23] = 0x20;	//MOTOR_RUN_STATE_RUNNING
	command[24] = 0x00;//run forever
	command[25] = 0x00;//run forever
	command[26] = 0x00;//run forever
	command[27] = 0x00;//run forever
	
	[manager sendRFCOMMPacket:command withLength:28];
}

- (Sensor *) sensorAt:(int)i {
	if (i >= 0 && i <4) {
		return sensors[i];
	}
	
	return nil;
}

//update sensor data
- (void) updateSensorData {
	char command[5];
	
	for (int i = 0; i < 4; i++) {
		if ([sensors[i] getType] != NO_SENSOR) {
			command[0] = 0x03;
			command[1] = 0x00;
			command[2] = 0x00;
			command[3] = 0x07;
			command[4] = (char) i;
			
			[manager sendRFCOMMPacket:command withLength:5];
		}
	}
}

- (void) btActivated {
	//bt activated, let's make connection
	[self connect];
}


- (void) btActivationFailed {
	NSLog(@"Activation btstack failed");
}

- (void) btDeactivated {
	NSLog(@"Btstack deactivated");
}

- (void) btDeviceConnected {
	
	char command[7];
	
	NSLog(@"Device connected");
	
	
	//send msg to device for register sensors!
	for (int i = 0; i < 4; i++) {
		if ([sensors[i] getType] != NO_SENSOR) {
			command[0] = 0x05;
			command[1] = 0x00;
			command[2] = 0x00;
			command[3] = 0x05;
			command[4] = (char) i;
			command[5] = (char) [sensors[i] getType];
			command[6] = (char) [sensors[i] getMode];
			
			[manager sendRFCOMMPacket:command withLength:7];
		}
	}
	
	//start timer
	timer = [NSTimer scheduledTimerWithTimeInterval:0.5 target:self selector:@selector(updateSensorData) userInfo:nil repeats:YES];
	
	//send message to observers
	[[NSNotificationCenter defaultCenter] postNotificationName:@"RobotDeviceConnected" object:nil];
}

- (void) btDeviceDisconnected {
	
	NSLog(@"Btstack disconnected");
	
	//stop send messages to sensor data update
	[timer invalidate];
	timer = nil;
	
	//send message to observers
	[[NSNotificationCenter defaultCenter] postNotificationName:@"RobotDeviceDisconnected" object:nil];
}

- (void) btReceivedD1:(uint8_t) d1 andD2: (uint8_t) d2 fromSensor:(int) sn {
	NSLog(@"Data %d %d from sensor %d received", (int)d1, (int)d2, sn);
	//set new data
	[sensors[sn] setData:d1 :d2];
	
	//send message to delegate
	[delegate sensorDataUpdated:sn];	
}

//SensorDataDelegate
- (void) setDelegate:(id <SensorDataDelegate>) d {
	delegate = d;
}

@end
